/*
 * Copyright (C) 2017 - 2019, AEye, Inc.
 * All rights reserved.
 */
#include <cstring>
#include <memory>

#include "spdlog/spdlog.h"

#include "HawkeyeFrame.hpp"
#include "SocketHawkeyeReader.hpp"
#include "Utils.hpp"

namespace aeye {

SocketHawkeyeReader::SocketHawkeyeReader(const std::string &sensorAddr, uint16_t clientPort, bool udpBroadcastMode)
: sensorAddress(sensorAddr)
, reader_(sensorAddr, clientPort, DEFAULT_SERVER_HAWKEYE_PORT, udpBroadcastMode)
, frame_queue_manager_(isRunning, maxQueueSize)
{ }

SocketHawkeyeReader::~SocketHawkeyeReader() noexcept {
  spdlog::trace("SHR dtor called.");
  if (isRunning) stop();
}

void SocketHawkeyeReader::run() {
  spdlog::trace("Started running socketHawkeyeReader.");
  set_thread_name(AEYE_LIT("HawkEyeRun"));
  isRunning           = true;

  auto buf = std::make_unique<uint8_t[]>(UDP_BUF_LEN);

  // Datagram source
  sockaddr_in src_addr{};

  // Try to read AEye frame
  // 30 mSec timeout = 30 fps UI update
  constexpr unsigned s = 3;
  constexpr unsigned us = 0;

  frame_queue_manager_.clean();

  while (isRunning) {
      fd_set readFlags{};

      if (!reader_.run_select(s, us, readFlags)) {
        continue;
      }

      const size_t bytesReceived = reader_.receive_bytes(buf.get(), src_addr);

      if (bytesReceived == 0) continue;

      // Timeout or wrong checksum
      if (!reader_.is_set(readFlags) || !isRunning) continue;

      if (!reader_.validate_ip_addr(src_addr)) continue;

      PacketSignalBuffer bufferReturn{};
      // non-UB way of creating struct from bytes
      std::memcpy(&bufferReturn, buf.get(), sizeof(bufferReturn));

      if (!Reader::find_marker(bufferReturn.header.marker)) {
        spdlog::warn("Encountered invalid marker in SHR.");
        continue;
      }
      if (bufferReturn.header.type != AEyeCommands::RET_ADC_BUFFER) {
        spdlog::warn("Encountered invalid type `{}` in SHR.", uint16_t{bufferReturn.header.type});
        continue;
      }
      // Do a version check
      if (bufferReturn.header.version!=RET_BUFFERS_VERSION) {
        spdlog::warn("Unsupported version of ADC Buffer packet received. Expecting version {}.", RET_BUFFERS_VERSION);
        continue;
      }
      frame_queue_manager_.push(HawkeyeFrame(sensorAddress, bufferReturn.body.grabId, bufferReturn.body.bufferType,
                                bufferReturn.body.buffer, bufferReturn.body.sweepInfo, bufferReturn.header.tsSec,
                                bufferReturn.header.tsuSec));
  }
}

void SocketHawkeyeReader::start() {
  reader_.start();
}

HawkeyeFrame SocketHawkeyeReader::pop() {
  return frame_queue_manager_.pop();
}

void SocketHawkeyeReader::stop() noexcept {
  if (!isRunning) return;
  isRunning = false;
  reader_.stop();
  spdlog::trace("Stopped SHR.");
}

}